//* Includes ------------------------------------------------------------------*/

// /* USER CODE BEGIN 0 */
// #include "stdlib.h"
// #include "stdarg.h"
// #include "string.h"
// #include "imu.h"
// #include <math.h>
// #include "oslib_config.h"
// #include "oslib.h"
// #include "../oslib/uart/oslib_uart.h"

// char IMU_RxBuff[256] = {0};
// IMUData_Packet_t IMUData_Packet;
// AHRSData_Packet_t AHRSData_Packet;

// float DATA_Trans(uint8_t Data_1, uint8_t Data_2, uint8_t Data_3, uint8_t Data_4)
// {
//   long long transition_32;
//   float tmp = 0;
//   int sign = 0;
//   int exponent = 0;
//   float mantissa = 0;
//   transition_32 = 0;
//   transition_32 |= Data_4 << 24;
//   transition_32 |= Data_3 << 16;
//   transition_32 |= Data_2 << 8;
//   transition_32 |= Data_1;
//   sign = (transition_32 & 0x80000000) ? -1 : 1;
//   exponent = ((transition_32 >> 23) & 0xff) - 127;
//   mantissa = 1 + ((float)(transition_32 & 0x7fffff) / 0x7fffff);
//   tmp = sign * mantissa * pow(2, exponent);
//   return tmp;
// }

// void imu_exe()
// {
//   //uuprintf("%d\n\r", UART2_TaskBufferSize);
//   osSemaphoreAcquire(UART2_RxSema, osWaitForever);
//   // uuprintf("%d\n\r", UART2_TaskBufferSize);
//   if (UART2_TaskBufferSize == 72)
//   {
//     for (int i = 0; i < UART2_TaskBufferSize; i++)
//     {
//       IMU_RxBuff[i] = UART2_TaskBuffer[i];
//     }
//     if (IMU_RxBuff[1] == TYPE_AHRS && IMU_RxBuff[2] == AHRS_LEN)
//     {
//       AHRSData_Packet.Roll = DATA_Trans(IMU_RxBuff[19], IMU_RxBuff[20], IMU_RxBuff[21], IMU_RxBuff[22]);    //横滚角
//       AHRSData_Packet.Pitch = DATA_Trans(IMU_RxBuff[23], IMU_RxBuff[24], IMU_RxBuff[25], IMU_RxBuff[26]);   //俯仰角
//       AHRSData_Packet.Heading = DATA_Trans(IMU_RxBuff[27], IMU_RxBuff[28], IMU_RxBuff[29], IMU_RxBuff[30]); //偏航角
//       uuprintf("AHRS: The Roll =  %f\r\n", AHRSData_Packet.Roll);
//       uuprintf("AHRS: The Pitch =  %f\r\n", AHRSData_Packet.Pitch);
//       uuprintf("AHRS: The Heading =  %f\r\n", AHRSData_Packet.Heading);
//     }
//     memset(IMU_RxBuff, 0x00, sizeof(IMU_RxBuff));
//   }
// }

#include "oslib.h"
#include "imu.h"
#include <math.h>
#include <string.h>
uint8_t ttl_receive;
uint8_t Fd_data[64];
uint8_t Fd_rsimu[64];
uint8_t Fd_rsahrs[56];
int rs_imutype = 0;
int rs_ahrstype = 0;
extern int Time_count;

IMUData_Packet_t IMUData_Packet;
AHRSData_Packet_t AHRSData_Packet;
char Usart_Receive[RxBufferSize];

void imu_exe()
{
  //static uint8_t Count = 0;
  //static uint8_t rs_count = 0;
  //static uint8_t last_rsnum = 0;
  // uint8_t Usart_Receive;
  //static uint8_t rsimu_flag = 0;
  //static uint8_t rsacc_flag = 0;

  ttl_receive = 1;
  if (osSemaphoreAcquire(UART2_RxSema, osWaitForever) == 0)
  {
    // uprintf("UART2_TaskBuffer %#X %#X %#X %#X \n\r", UART2_TaskBuffer[0], UART2_TaskBuffer[1], UART2_TaskBuffer[2], UART2_TaskBuffer[3]);
    memcpy(Usart_Receive, UART2_TaskBuffer, sizeof(UART2_TaskBuffer));
  }
  if (Usart_Receive[0] == FRAME_HEAD)
  {
    if (Usart_Receive[1] == TYPE_AHRS && Usart_Receive[2] == AHRS_LEN)
    {
      if (Usart_Receive[AHRS_RS - 1] == FRAME_END)
      {
        memcpy(Fd_rsahrs, Usart_Receive, sizeof(Fd_rsahrs));
        rs_ahrstype = 1;
      }
    }
    // uprintf("Usart_Receive[AHRS_RS] %#x  \n\r", Usart_Receive[AHRS_RS]);
    // uprintf("Usart_Receive[AHRS_RS + IMU_RS - 1]  %#x\n\r", Usart_Receive[AHRS_RS + IMU_RS - 1]);
    if (Usart_Receive[AHRS_RS] == FRAME_HEAD && Usart_Receive[AHRS_RS + 1] == TYPE_IMU && Usart_Receive[AHRS_RS + 2] == IMU_LEN)
    {
      // uprintf("%#x\n\r", Usart_Receive[AHRS_RS + IMU_RS - 1]);
      if (Usart_Receive[AHRS_RS + IMU_RS - 1] == FRAME_END)
      {
        memcpy(Fd_rsimu, Usart_Receive + sizeof(Fd_rsahrs), sizeof(Fd_rsimu));
        rs_imutype = 1;
      }
    }
  }
  TTL_Hex2Dec();
}
/*************
实现16进制的can数据转换成浮点型数据
****************/
float DATA_Trans(uint8_t Data_1, uint8_t Data_2, uint8_t Data_3, uint8_t Data_4)
{
  long long transition_32;
  float tmp = 0;
  int sign = 0;
  int exponent = 0;
  float mantissa = 0;
  transition_32 = 0;
  // uprintf("\n\r\n\rtransition_32 %lld \n\r", transition_32);
  transition_32 |= Data_4 << 24;
  // uprintf("transition_32 %lld \n\r", transition_32);
  transition_32 |= Data_3 << 16;
  // uprintf("transition_32 %lld \n\r", transition_32);
  transition_32 |= Data_2 << 8;
  // uprintf("transition_32 %lld \n\r", transition_32);
  transition_32 |= Data_1;
  // uprintf("transition_32 %lld \n\r", transition_32);
  sign = (transition_32 & 0x80000000) ? -1 : 1; //符号位
  // uprintf("sign %d \n\r", sign);
  //先右移操作，再按位与计算，出来结果是30到23位对应的e
  exponent = ((transition_32 >> 23) & 0xff) - 127;
  // uprintf("exponent %d \n\r", exponent);
  //将22~0转化为10进制，得到对应的x系数
  mantissa = 1 + ((float)(transition_32 & 0x7fffff) / (float)0x7fffff);
  // uprintf("mantissa %f \n\r", mantissa);
  tmp = sign * mantissa * pow(2, exponent);
  // uprintf("tmp %f \n\r\n\r", tmp);
  return tmp;
}

/*******************************
16进制转浮点型数据
*******************************/
uint8_t TTL_Hex2Dec(void)
{
  if (rs_ahrstype == 1)
  {
    if (Fd_rsahrs[1] == TYPE_AHRS && Fd_rsahrs[2] == AHRS_LEN)
    {
      AHRSData_Packet.RollSpeed = DATA_Trans(Fd_rsahrs[7], Fd_rsahrs[8], Fd_rsahrs[9], Fd_rsahrs[10]);       //横滚角速度
      AHRSData_Packet.PitchSpeed = DATA_Trans(Fd_rsahrs[11], Fd_rsahrs[12], Fd_rsahrs[13], Fd_rsahrs[14]);   //俯仰角速度
      AHRSData_Packet.HeadingSpeed = DATA_Trans(Fd_rsahrs[15], Fd_rsahrs[16], Fd_rsahrs[17], Fd_rsahrs[18]); //偏航角速度

      AHRSData_Packet.Roll = DATA_Trans(Fd_rsahrs[19], Fd_rsahrs[20], Fd_rsahrs[21], Fd_rsahrs[22]);    //横滚角
      AHRSData_Packet.Pitch = DATA_Trans(Fd_rsahrs[23], Fd_rsahrs[24], Fd_rsahrs[25], Fd_rsahrs[26]);   //俯仰角
      AHRSData_Packet.Heading = DATA_Trans(Fd_rsahrs[27], Fd_rsahrs[28], Fd_rsahrs[29], Fd_rsahrs[30]); //偏航角

      AHRSData_Packet.Qw = DATA_Trans(Fd_rsahrs[31], Fd_rsahrs[32], Fd_rsahrs[33], Fd_rsahrs[34]); //四元数
      AHRSData_Packet.Qx = DATA_Trans(Fd_rsahrs[35], Fd_rsahrs[36], Fd_rsahrs[37], Fd_rsahrs[38]);
      AHRSData_Packet.Qy = DATA_Trans(Fd_rsahrs[39], Fd_rsahrs[40], Fd_rsahrs[41], Fd_rsahrs[42]);
      AHRSData_Packet.Qz = DATA_Trans(Fd_rsahrs[43], Fd_rsahrs[44], Fd_rsahrs[45], Fd_rsahrs[46]);
      AHRSData_Packet.Timestamp = timestamp(Fd_rsahrs[47], Fd_rsahrs[48], Fd_rsahrs[49], Fd_rsahrs[50]); //时间戳
      PrintAHRSData();
    }
    rs_ahrstype = 0;
  }
  if (rs_imutype == 1)
  {
    if (Fd_rsimu[1] == TYPE_IMU && Fd_rsimu[2] == IMU_LEN)
    {
      IMUData_Packet.gyroscope_x = DATA_Trans(Fd_rsimu[7], Fd_rsimu[8], Fd_rsimu[9], Fd_rsimu[10]); //角速度
      IMUData_Packet.gyroscope_y = DATA_Trans(Fd_rsimu[11], Fd_rsimu[12], Fd_rsimu[13], Fd_rsimu[14]);
      IMUData_Packet.gyroscope_z = DATA_Trans(Fd_rsimu[15], Fd_rsimu[16], Fd_rsimu[17], Fd_rsimu[18]);

      IMUData_Packet.accelerometer_x = DATA_Trans(Fd_rsimu[19], Fd_rsimu[20], Fd_rsimu[21], Fd_rsimu[22]); //线加速度
      IMUData_Packet.accelerometer_y = DATA_Trans(Fd_rsimu[23], Fd_rsimu[24], Fd_rsimu[25], Fd_rsimu[26]);
      IMUData_Packet.accelerometer_z = DATA_Trans(Fd_rsimu[27], Fd_rsimu[28], Fd_rsimu[29], Fd_rsimu[30]);

      IMUData_Packet.magnetometer_x = DATA_Trans(Fd_rsimu[31], Fd_rsimu[32], Fd_rsimu[33], Fd_rsimu[34]); //磁力计数据
      IMUData_Packet.magnetometer_y = DATA_Trans(Fd_rsimu[35], Fd_rsimu[36], Fd_rsimu[37], Fd_rsimu[38]);
      IMUData_Packet.magnetometer_z = DATA_Trans(Fd_rsimu[39], Fd_rsimu[40], Fd_rsimu[41], Fd_rsimu[42]);

      IMUData_Packet.Timestamp = timestamp(Fd_rsimu[55], Fd_rsimu[56], Fd_rsimu[57], Fd_rsimu[58]); //时间戳
      PrintIMUData();
    }
    rs_imutype = 0;
  }
  return 0;
}

void PrintAHRSData(void)
{
  uprintf("AHRS: The RollSpeed =  %f\r\n", AHRSData_Packet.RollSpeed);
  uprintf("AHRS: The PitchSpeed =  %f\r\n", AHRSData_Packet.PitchSpeed);
  uprintf("AHRS: The HeadingSpeed =  %f\r\n", AHRSData_Packet.HeadingSpeed);
  uprintf("AHRS: The Roll =  %f\r\n", AHRSData_Packet.Roll);
  uprintf("AHRS: The Pitch =  %f\r\n", AHRSData_Packet.Pitch);
  uprintf("AHRS: The Heading =  %f\r\n", AHRSData_Packet.Heading);
  uprintf("AHRS: The Quaternion.Qw =  %f\r\n", AHRSData_Packet.Qw);
  uprintf("AHRS: The Quaternion.Qx =  %f\r\n", AHRSData_Packet.Qx);
  uprintf("AHRS: The Quaternion.Qy =  %f\r\n", AHRSData_Packet.Qy);
  uprintf("AHRS: The Quaternion.Qz =  %f\r\n", AHRSData_Packet.Qz);
  uprintf("AHRS: The Timestamp =  %d\r\n", AHRSData_Packet.Timestamp);
  uprintf("\r\n");
}
void PrintIMUData(void)
{
  // uprintf("Now start sending IMU data.\r\n");
  uprintf("IMU: The gyroscope_x =  %f\r\n", IMUData_Packet.gyroscope_x);
  uprintf("IMU:The gyroscope_y =  %f\r\n", IMUData_Packet.gyroscope_y);
  uprintf("IMU:The gyroscope_z =  %f\r\n", IMUData_Packet.gyroscope_z);
  uprintf("IMU:The accelerometer_x =  %f\r\n", IMUData_Packet.accelerometer_x);
  uprintf("IMU:The accelerometer_y =  %f\r\n", IMUData_Packet.accelerometer_y);
  uprintf("IMU:The accelerometer_z =  %f\r\n", IMUData_Packet.accelerometer_z);
  uprintf("IMU:The magnetometer_x =  %f\r\n", IMUData_Packet.magnetometer_x);
  uprintf("IMU:The magnetometer_y =  %f\r\n", IMUData_Packet.magnetometer_y);
  uprintf("IMU:The magnetometer_z =  %f\r\n", IMUData_Packet.magnetometer_z);
  uprintf("\r\n");
  // uprintf("Now the data of IMU has been sent.\r\n");
}

long long timestamp(uint8_t Data_1, uint8_t Data_2, uint8_t Data_3, uint8_t Data_4)
{
  uint32_t transition_32;
  transition_32 = 0;
  transition_32 |= Data_4 << 24;
  transition_32 |= Data_3 << 16;
  transition_32 |= Data_2 << 8;
  transition_32 |= Data_1;
  return transition_32;
}